//#include <imu_process.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/Imu.h>
#include <dynamixel_msgs/MotorStateList.h>
#include <dynamixel_msgs/MotorState.h>
#include <mother_ship/DirectionControl.h>
#include <mother_ship/IRArray.h>
#include <mother_ship/MyIMU.h>
#include <mother_ship/ModeControl.h>
#include <nav_msgs/Odometry.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <encoder/coords.h>
using namespace std;

#define FORWARD 0
#define BACKWARD 1
#define LEFT 2
#define RIGHT 3
#define PI 3.1415926

// ros communication functions
void keyboardContrl_Callback(const std_msgs::String::ConstPtr &msg);
void irData_Callback(const mother_ship::IRArray::ConstPtr &msg);
void imu_my_Callback(const mother_ship::MyIMU::ConstPtr &msg);
void Odom_Callback(const encoder::coords::ConstPtr &msg);
void mag_Callback(const std_msgs::Float32::ConstPtr &msg);
void directionControl_Callback(
		const mother_ship::DirectionControl::ConstPtr &msg);
void modeControl_Callback(const mother_ship::ModeControl::ConstPtr &msg);
void controlCommand(string cmd);
void init_Pub_Sub(ros::NodeHandle &n);
void traction_Callback(const dynamixel_msgs::MotorStateList::ConstPtr &msgs);
void get_ids();
void set_Servo_Limit();
// initialization configuration
void getParameters(ros::NodeHandle &n);
void init_Servo_Config();
void set_Servo_Config(struct servo_configuration);

// angles for four dynamixel motors is a configuration
struct servo_configuration {
	double ri, ro, li, lo;
};
struct servo_ids_configuration {
	int ri, ro, li, lo;
};
// configurations for different move direction
// initialization is in init_Servo_config
// 0 Forward 1 Backward 2 Left 3 Right
struct servo_configuration tilt_config[4];
struct servo_configuration trans_config[4];
struct servo_configuration start_config;
struct servo_configuration current_config;
struct servo_configuration max_config;
struct servo_configuration min_config;
struct servo_ids_configuration servo_ids;
// ros communication
ros::Subscriber keyboard_cmd;
ros::Subscriber ir_sub;
ros::Subscriber local_direct_sub;
ros::Subscriber imu_sub;
ros::Subscriber mag_sub;
ros::Subscriber mode_control_sub;
ros::Publisher ir_data;
ros::Publisher ir_state_pub;
ros::Publisher servo1_cmd;
ros::Publisher servo2_cmd;
ros::Publisher servo3_cmd;
ros::Publisher servo4_cmd;
ros::Publisher dc_right_speed;
ros::Publisher dc_left_speed;
ros::Publisher globalDirect_pub;
ros::Publisher detection_interval_pub;
ros::Publisher detect_once_pub;
std_msgs::Int32MultiArray ir_msg;
std_msgs::Float32MultiArray servo_msg;
std_msgs::Int32 dc_msg;
ros::Subscriber Gimbal_Feedback;
ros::Subscriber Odom_sub;

float Corridor_dist;
float Corridor_angle;
float Local_desired;
bool stopped;
double traction_upbound, traction_lowbound, traction_middle, servo_angle_step;

// value for speed control, now is degenerated to control the same
// havent change code yet
int right_speed_go, right_speed_stop, left_speed_go, left_speed_stop,
		right_speed, left_speed;
int right_speed_last, left_speed_last;
int max_speed_scale, speed_diff_scale;
int right_motor_base_speed, left_motor_base_speed;

double turn_time;
double interval_a, interval_b;
double safe_dist;
// direction control parameter
bool use_mag;
bool use_encoder;

// which configuration it is now
int robot_state; //

// movement tasks
bool corridor_following, turning, tagtracking;

int global_desired_direction, current_global_direction;

// configuration sequence for transformation between directions
int trans_seq[40];

// constants for encoder angle
double dl, dr, d, vl, vr, x, y, th, thp;

int ir_state[4];
int in_transition;
int main(int argc, char** argv) {
	ros::init(argc, argv, "mother_ship");
	ros::NodeHandle n;

	getParameters(n);
	init_Servo_Config();
	init_Pub_Sub(n);

	right_speed_stop = 90;
	left_speed_stop = 90;
	right_speed_go = 100;
	left_speed_go = 100;

	right_speed = 90;
	left_speed = 90;
	right_speed_last = 90;
	left_speed_last = 90;

	// 0 forward 1 backward 2 right 3 left
	robot_state = -1;
	corridor_following = false;
	turning = false;
	tagtracking = false;
	global_desired_direction = -1;
	in_transition = 1;
	set_Servo_Config(start_config);

	// set the IDs of the servos

	// Constants for Encoder angle
	//dl = 0.0;
	//dr = 0.0;
	//d = 0.0;

	x = 0.0;
	y = 0.0;
	th = PI / 2.0;
	//thp = 0.0;

	// when distance is 5.0 , 3 seconds detect once
	// when at safe distance, 0.5 second detect once
	interval_a = (15.0 - 0.5 * safe_dist) / 2.5;
	interval_b = 15.0 - 3.0 * interval_a;

	stopped = true;

	get_ids();
	//set the max position limits of the servos
	set_Servo_Limit();

	// Main Code
	ros::spin();
}
void getParameters(ros::NodeHandle &n) {

	n.param("use_mag", use_mag, false);
	n.param("use_encoder", use_encoder, false);
	//n.param("track_apriltag", tagtracking, false);
	n.param("max_speed_scale", max_speed_scale, 30);
	n.param("speed_difference_scale", speed_diff_scale, 60);
	n.param("right_motor_base_speed", right_motor_base_speed, 92);
	n.param("left_motor_base_speed", left_motor_base_speed, 89);
	n.param("turn_time", turn_time, 0.5);
	n.param("safe_distance", safe_dist, 2.5);

	//double traction_upbound, traction_lowbound, traction_middle, servo_angle_step;
	n.param("traction_upbound", traction_upbound, 0.16);
	n.param("traction_lowbound", traction_lowbound, 0.14);
	n.param("traction_middle", traction_middle, 0.15);
	n.param("servo_angle_step", servo_angle_step, 0.0025);

	//n->param("laser_scan_frame", scan_frame_name, std::string("laser"));
	//n->param("inflation_radius", inflation_radius, 0.5);
}
void init_Pub_Sub(ros::NodeHandle &n) {
	// All Subscriber
	// --- Keyboard Control
	keyboard_cmd = n.subscribe("Keyboard_Control", 1, keyboardContrl_Callback);
	// IR Sensor
	//ir_sub = n.subscribe("IRSensor", 1, irData_Callback);
	// IMU
	imu_sub = n.subscribe("imu_my", 1, imu_my_Callback);
	// Magnetometer
	mag_sub = n.subscribe<std_msgs::Float32> ("Magnetometer", 1, mag_Callback);
	// Local Planning
	local_direct_sub = n.subscribe("direction_control", 1,
			directionControl_Callback);
	// Mode Control
	mode_control_sub = n.subscribe("mode_control", 5, modeControl_Callback);
	// All Publisher
	// IR Sensor
	//ir_data = n.advertise<std_msgs::Int32MultiArray> ("IR_Data", 1);
	ir_msg.data.resize(5);
	// 0 far 1 close 2 near
	//ir_state_pub = n.advertise<std_msgs::String> ("IR_State", 1);
	for (int i = 0; i < 4; i++)
		ir_state[i] = 0;
	// IMU

	// Servo Motor
	servo1_cmd = n.advertise<std_msgs::Float64> ("right_inner/command", 2);
	servo2_cmd = n.advertise<std_msgs::Float64> ("right_outer/command", 2);
	servo3_cmd = n.advertise<std_msgs::Float64> ("left_inner/command", 2);
	servo4_cmd = n.advertise<std_msgs::Float64> ("left_outer/command", 2);
	// DC Motor
	dc_right_speed = n.advertise<std_msgs::Int32> ("DC_Motor_Right_Speed", 1);
	dc_left_speed = n.advertise<std_msgs::Int32> ("DC_Motor_Left_Speed", 1);
	Gimbal_Feedback = n.subscribe("motor_states/pan_tilt_port", 1,
			traction_Callback);
	globalDirect_pub = n.advertise<std_msgs::Float32> ("GlobalDirection", 1);
	Odom_sub = n.subscribe("odom_pub", 1, Odom_Callback);
	detection_interval_pub = n.advertise<std_msgs::Float32> (
			"detection_interval_change", 2);
	detect_once_pub = n.advertise<std_msgs::Int32> ("detection_once", 2);
}
void modeControl_Callback(const mother_ship::ModeControl::ConstPtr &msg) {
	if (msg->mode == "TagTracking") {
		Corridor_angle = msg->desired_relative_angle + current_global_direction;
		tagtracking = true;
		return;
	}
	tagtracking = false;
	string cmd;
	cmd = "STOP";
	controlCommand(cmd);
	//sleep(1);

	if (msg->mode == "CorridorFollowing") {
		tagtracking = false;
		global_desired_direction = msg->desired_global_angle;
		cmd = "CORFOLLOW";
		// desireddirection
		std_msgs::Float32 new_msg;
		if (!use_mag)
			new_msg.data = 0.0;
		else
			new_msg.data = global_desired_direction - current_global_direction; //-diff_angle / 180.0 * PI;
		globalDirect_pub.publish(new_msg);
		// send to local map control
		ROS_INFO("Switch to Corridor Following Mode");
		controlCommand(cmd);
		return;
	}
	if (msg->mode == "Turn") {
		ROS_INFO("Switch to Turn Mode");
		tagtracking = false;
		// start to turn
		// turn left
		// calculate relative angle ***********
		if (msg->desired_relative_angle > 0) {
			ROS_INFO("Start to turn left to next place");
			dc_msg.data = left_speed_stop;
			left_speed = left_speed_stop;
			dc_left_speed.publish(dc_msg);
			dc_msg.data = right_speed_go;
			right_speed = right_speed_go;
			dc_right_speed.publish(dc_msg);
			Corridor_angle = current_global_direction + msg->desired_relative_angle;
		}
		// turn right
		else {
			ROS_INFO("Start to turn right to next place");
			dc_msg.data = right_speed_stop;
			right_speed = right_speed_stop;
			dc_right_speed.publish(dc_msg);
			dc_msg.data = left_speed_go + 2;
			left_speed = left_speed_go + 2;
			dc_left_speed.publish(dc_msg);
			Corridor_angle = current_global_direction + msg->desired_relative_angle;
		}

		if (!use_encoder) {
			// accelerate one wheel for a short time and then go back to corridor following
			sleep(turn_time);
			cmd = "STOP";
			controlCommand(cmd);
			ROS_INFO("Finished Turning, Switch to Corridor Following");
			std_msgs::Float32 new_msg;
			new_msg.data = 0.0;
			globalDirect_pub.publish(new_msg);
			// Switch to corridor following
			cmd = "CORFOLLOW";
			controlCommand(cmd);
		}
		// use turning control
		else {
			//global_desired_direction = msg->desired_global_angle;
			turning = true;
		}
		return;
	}
	if (msg->mode == "Arrived") {
		ROS_INFO("Arrived Destination! Life Saved!");
		cmd = "STOP";
		controlCommand(cmd);
		return;
	}
	if (msg->mode == "Stop") {
		ROS_INFO("Force to stop! Maybe go to wrong place.");
		cmd = "STOP";
		controlCommand(cmd);
		return;
	}
}

void directionControl_Callback(
		const mother_ship::DirectionControl::ConstPtr &msg) {
	std_msgs::Float32 detect_msg;
	if (!corridor_following) {
		return;
	}
	if (tagtracking) {
		Corridor_dist = 2.0 < msg->dist_front ? 2.0 : msg->dist_front;
		detect_msg.data = 0.3;
		detection_interval_pub.publish(detect_msg);
		return;
	}
	//return;
	Corridor_dist = msg->dist_front;
	Corridor_angle = current_global_direction + msg->desired_angle_offset;
	//cout<<Corridor_angle;
	// angle > 0 : turn left,angle < 0 : turn right

	//detect_msg.data = (float)(interval_a / Corridor_dist + interval_b);
	if (Corridor_dist >= 4.0)
		detect_msg.data = 4.0;
	else if (Corridor_dist >= 3.0)
		detect_msg.data = 2.5;
	else if (Corridor_dist >= 2.0)
		detect_msg.data = 1.8;
	//else if (Corridor_dist >= 1.0)
	//	detect_msg.data = 0.8;
	else
		detect_msg.data = 0.5;
	detection_interval_pub.publish(detect_msg);

	if (stopped && Corridor_dist > safe_dist) {
		right_speed_last = right_speed_go;
		left_speed_last = left_speed_go;
		dc_msg.data = left_speed_last;
		dc_left_speed.publish(dc_msg);
		dc_msg.data = right_speed_last;
		dc_right_speed.publish(dc_msg);
		stopped = false;
	}
}

void Odom_Callback(const encoder::coords::ConstPtr &msg) {
	if (!use_encoder)
		return;

	//dl = -(double)(msg->l_relpos) / 800.0*0.048 * PI; // m
	//dr = -(double)(msg->r_relpos) / 800.0*0.048 * PI; // m

	//double l = 0.3175; 

	//x = x + d * cos(th + thp/2.0);
	//y = y + d * sin(th + thp/2.0);
	//thp = (dr - dl) / l;
	//d = (dr + dl) / 2.0;
	//th = th + thp;

	x = msg->x;
	y = msg->y;
	th = msg->th;

	current_global_direction = th; //th/PI*180;

	//cout << "\n" << msg->D;
	//cout << "\n" << sqrt(x*x+y*y);
	//	cout << "\nx:" << x << " y:" << y << " th:" << current_global_direction;
	//cout << "  Desireout Angle:"<< Corridor_angle;

	// if in turn
	if (turning) {
		// if turned enough angle then start to follow corridor
		if (std::abs(current_global_direction - Corridor_angle) < 5) {
			turning = false;
			string cmd;
			cmd = "STOP";
			controlCommand(cmd);
			ROS_INFO("Finished Turning, Switch to Corridor Following");
			// global_desired_direction
			std_msgs::Float32 new_msg;
			new_msg.data = global_desired_direction - current_global_direction;
			globalDirect_pub.publish(new_msg);
			// send to local planner
			cmd = "CORFOLLOW";
			controlCommand(cmd);
		}
		return;
	}
	if (corridor_following || tagtracking) {
		int right_speed, left_speed;
		if (Corridor_dist < safe_dist) {
			//ROS_INFO("Too Close only %.3lf to %.3lf.", Corridor_dist, safe_dist);
			right_speed = right_speed_stop;
			left_speed = left_speed_stop;
			stopped = true;
		} else {
			double ang_diff = (Corridor_angle - current_global_direction);
			//cout<<"\n"<<ang_diff;
			if (abs(ang_diff) > 1.0) {
				float max_speed = Corridor_dist / 5.0 * max_speed_scale;
				right_speed = int(((ang_diff + speed_diff_scale) * max_speed)
						/ (speed_diff_scale + 3) / 2) + right_motor_base_speed;
				left_speed = int(((speed_diff_scale - ang_diff) * max_speed)
						/ (speed_diff_scale - 4) / 2) + left_motor_base_speed;
				right_speed = (right_speed + right_speed_last) / 2.0;
				left_speed = (left_speed + left_speed_last) / 2.0;
				//cout<<right_speed<<"!"<<left_speed<<"!"<< ang_diff;
			} else {
				right_speed = right_speed_go;
				left_speed = left_speed_go;
			}
		}
		//ROS_INFO("%.2lf %d %d %d %d", Corridor_angle - current_global_direction, Corridor_angle, current_global_direction, max_speed_scale, speed_diff_scale);
		//cout<<"<5R"<<right_speed<<"-L"<<left_speed;
		right_speed = right_speed > 150 ? 150 : right_speed;
		left_speed = left_speed > 150 ? 150 : left_speed;
		//:cout<<">R"<<right_speed<<"-L"<<left_speed;

		//ROS_INFO("Corridor_distance %f angle %f r_speed %d l_speed %d", dist, angle, right_speed, left_speed);
		if (right_speed_last != right_speed) {
			right_speed_last = right_speed;
			// set right speed
			dc_msg.data = right_speed_last;
			dc_right_speed.publish(dc_msg);
		}
		if (left_speed_last != left_speed) {
			left_speed_last = left_speed;
			// set left speed
			dc_msg.data = left_speed_last;
			dc_left_speed.publish(dc_msg);
		}

		std_msgs::Float32 new_msg;
		new_msg.data = global_desired_direction - current_global_direction;
		globalDirect_pub.publish(new_msg);
	}
}

void mag_Callback(const std_msgs::Float32::ConstPtr &msg) {
	if (!use_mag)
		return;
	current_global_direction = msg->data;
	// if in turn
	if (turning) {
		// if turned enough angle then start to follow corridor
		if (std::abs(current_global_direction - global_desired_direction) < 10) {
			turning = false;
			string cmd;
			cmd = "STOP";
			controlCommand(cmd);
			ROS_INFO("Finished Turning, Switch to Corridor Following");
			// global_desired_direction
			std_msgs::Float32 new_msg;
			new_msg.data = global_desired_direction - current_global_direction;
			globalDirect_pub.publish(new_msg);
			// send to local planner
			cmd = "CORFOLLOW";
			controlCommand(cmd);
		}
		return;
	}
	if (corridor_following) {
		std_msgs::Float32 new_msg;
		new_msg.data = global_desired_direction - current_global_direction;
		globalDirect_pub.publish(new_msg);
	}
}

void controlCommand(string cmd) {
	int robot_prev = robot_state;
	int flag = 10;
	if (cmd == "FORWARD")
		flag = robot_state = 0;
	if (cmd == "BACKWARD")
		flag = robot_state = 1;
	if (cmd == "LEFT")
		flag = robot_state = 2;
	if (cmd == "RIGHT")
		flag = robot_state = 3;
	if (cmd == "LIFT" && robot_prev != -1) {
		flag = robot_state = -1;
		in_transition = 1;
		set_Servo_Config(start_config);
	}
	if (cmd == "CORFOLLOW" && !corridor_following) {
		puts("Following Corridors Switch on");
		Corridor_dist = 0.0;
		corridor_following = true;
		stopped = false;
		flag = robot_state = 0;
	} else {
		puts("Following Corridors Switch off");
		corridor_following = false;
	}

	if (robot_prev != robot_state) {
		in_transition = 1;
	} else {
		in_transition = 0;
	}

	// start transformation
	if ((flag != 10) && (in_transition) && (robot_state != -1) && (robot_prev
			!= -1)) {
		int temp = trans_seq[robot_prev * 10 + robot_state];
		cout << "From " << robot_prev << " to " << robot_state << endl;
		while (temp > 0) {
			set_Servo_Config(trans_config[temp % 10 - 1]);
			cout << "Turn Transition = " << temp % 10 << endl;
			temp /= 10;
			int i = 0;
			sleep(0.5);
			in_transition = 0;
		}
		set_Servo_Config(tilt_config[robot_state]);
		in_transition = 0;
	} else if ((flag != 10) && (robot_prev == -1)) {
		set_Servo_Config(tilt_config[robot_state]);
		in_transition = 0;
	}

	// speed control
	// left or right are both start at the same time
	if (cmd == "START" || corridor_following) {//"R_START" || cmd == "L_START") {
		dc_msg.data = right_speed_go;
		right_speed = right_speed_go;
		dc_msg.data = left_speed_go;
		left_speed = left_speed_go;
		dc_right_speed.publish(dc_msg);
		dc_left_speed.publish(dc_msg);
		return;
	}
	// when any motor is commanded to stop coridor follwing is stopped the robot is then in forward state
	if (cmd == "STOP") {//"R_STOP" || cmd == "L_STOP" || cmd == "STOP") {
		corridor_following = false;
		dc_msg.data = right_speed_stop;
		right_speed = right_speed_stop;
		dc_msg.data = left_speed_stop;
		left_speed = left_speed_stop;
		dc_right_speed.publish(dc_msg);
		dc_left_speed.publish(dc_msg);
		return;
	}
	// increase or decrease
	if (cmd == "R_I") {
		right_speed += 1;
		dc_msg.data = right_speed;
		dc_right_speed.publish(dc_msg);
		return;
	}
	if (cmd == "R_D") {
		right_speed -= 1;
		dc_msg.data = right_speed;
		dc_right_speed.publish(dc_msg);
		return;
	}
	if (cmd == "L_I") {
		left_speed += 1;
		dc_msg.data = left_speed;
		dc_left_speed.publish(dc_msg);
		return;
	}
	if (cmd == "L_D") {
		left_speed -= 1;
		dc_msg.data = left_speed;
		dc_left_speed.publish(dc_msg);
		return;
	}
}
void keyboardContrl_Callback(const std_msgs::String::ConstPtr &msg) {
	// all controls are projected to keyboard
	// check keyboard_ctrl package for projection
	string cmd = msg->data;
	controlCommand(cmd);
}
void get_ids() {
	// set which part of the dignostic msg each corresponds to
	servo_ids.ri = 3;
	servo_ids.ro = 0;
	servo_ids.li = 1;
	servo_ids.lo = 2;
	cout << "id_Set";
}
void set_Servo_Limit() {
	max_config.ri = 0.7;
	max_config.li = 0.7;
	max_config.ro = 0.7;
	max_config.lo = 0.7;
	min_config.ri = 0.35;
	min_config.li = 0.35;
	min_config.ro = 0.35;
	min_config.lo = 0.35;

}

void set_Servo_Config(struct servo_configuration config) {
	std_msgs::Float64 msg;
	msg.data = config.ri;
	servo1_cmd.publish(msg);
	msg.data = config.ro;
	servo2_cmd.publish(msg);
	msg.data = config.li;
	servo3_cmd.publish(msg);
	msg.data = config.lo;
	servo4_cmd.publish(msg);
	current_config = config;

}
void init_Servo_Config() {
	start_config.ri = 0.75;
	start_config.ro = 0.0;
	start_config.li = -0.75;
	start_config.lo = 0.0;
	// 0 Forward 1 Backward 2 Left 3 Right
	// Please note that these have been changed
	tilt_config[0].ri = 0.55;
	tilt_config[0].ro = 0.0;
	tilt_config[0].li = -0.55;
	tilt_config[0].lo = 0.0;
	tilt_config[1].ri = -0.55;
	tilt_config[1].ro = 0.0;
	tilt_config[1].li = 0.55;
	tilt_config[1].lo = 0.0;
	tilt_config[2].ri = 0.0;
	tilt_config[2].ro = -0.55;
	tilt_config[2].li = 0.0;
	tilt_config[2].lo = -0.55;
	tilt_config[3].ri = 0.0;
	tilt_config[3].ro = 0.55;
	tilt_config[3].li = 0;
	tilt_config[3].lo = 0.55;
	// 0 FR 1 FL 2 BR 3 BL
	trans_config[0].ri = 0.65;
	trans_config[0].ro = 0.65;
	trans_config[0].li = -0.65;
	trans_config[0].lo = 0.65;
	trans_config[1].ri = 0.65;
	trans_config[1].ro = -0.65;
	trans_config[1].li = -0.65;
	trans_config[1].lo = -0.65;
	trans_config[2].ri = -0.65;
	trans_config[2].ro = 0.65;
	trans_config[2].li = 0.65;
	trans_config[2].lo = 0.65;
	trans_config[3].ri = -0.65;
	trans_config[3].ro = -0.65;
	trans_config[3].li = 0.65;
	trans_config[3].lo = -0.65;
	// f - b, r, l
	trans_seq[0] = 0;
	trans_seq[1] = 42;
	trans_seq[2] = 2;
	trans_seq[3] = 1;
	// b - f, r, l
	trans_seq[10] = 13;
	trans_seq[11] = 0;
	trans_seq[12] = 4;
	trans_seq[13] = 3;
	// r - f, b, l
	trans_seq[20] = 2;
	trans_seq[21] = 4;
	trans_seq[22] = 0;
	trans_seq[23] = 12;
	// l - f, b, r
	trans_seq[30] = 1;
	trans_seq[31] = 3;
	trans_seq[32] = 43;
	trans_seq[33] = 0;
}
void traction_Callback(const dynamixel_msgs::MotorStateList::ConstPtr &msgs) {
	//traction control in the forward direction
	if (robot_state == 0 && in_transition == 0) {
		//cout<<"\nf:ri:"<<msgs->motor_states[servo_ids.ri].load;
		//cout<<"\nf:li:"<<msgs->motor_states[servo_ids.li].load;
		if ((msgs->motor_states[servo_ids.ri].load < traction_lowbound)
				|| (msgs->motor_states[servo_ids.ri].load > traction_upbound)) {
			if ((abs(current_config.ri) < abs(max_config.ri)) && (abs(
					current_config.ri) > abs(min_config.ri))) {
				if ((msgs->motor_states[servo_ids.ri].load - traction_middle)
						> 0) {
					current_config.ri = current_config.ri + servo_angle_step;

				} else {
					current_config.ri = current_config.ri - servo_angle_step;
				}
			}
		}

		if (msgs->motor_states[servo_ids.li].load > -traction_lowbound
				|| msgs->motor_states[servo_ids.li].load < -traction_upbound) {
			if ((abs(current_config.li) < abs(max_config.li)) && (abs(
					current_config.li) > abs(min_config.li))) {
				if ((msgs->motor_states[servo_ids.li].load + traction_middle)
						> 0) {
					current_config.li = current_config.li + servo_angle_step;
				} else {
					current_config.li = current_config.li - servo_angle_step;
				}
			}
		}
		set_Servo_Config(current_config);

	}
	//traction control in the reverse direction
	else if (robot_state == 1 && in_transition == 0) {
		//		cout<<"\nb:ri:"<<msgs->motor_states[servo_ids.ri].load;
		//	cout<<"\nb:li:"<<msgs->motor_states[servo_ids.li].load;
		if (msgs->motor_states[servo_ids.ri].load > -traction_lowbound
				|| msgs->motor_states[servo_ids.ri].load < -traction_upbound) {
			if ((abs(current_config.ri) < abs(max_config.ri)) && (abs(
					current_config.ri) > abs(min_config.ri))) {
				if ((msgs->motor_states[servo_ids.ri].load + traction_middle)
						> 0) {
					current_config.ri = current_config.ri + servo_angle_step;
				} else {
					current_config.ri = current_config.ri - servo_angle_step;
				}
			}
		}

		if (msgs->motor_states[servo_ids.li].load < traction_lowbound
				|| msgs->motor_states[servo_ids.li].load > traction_upbound) {
			if ((abs(current_config.li) < abs(max_config.li)) && (abs(
					current_config.li) > abs(min_config.li))) {
				if ((msgs->motor_states[servo_ids.li].load - traction_middle)
						> 0) {
					current_config.li = current_config.li + servo_angle_step;
				} else {
					current_config.li = current_config.li - servo_angle_step;
				}
			}
		}
		set_Servo_Config(current_config);

	}
	//traction control in the left direction
	else if (robot_state == 2 && in_transition == 0) {
		//cout << "\nb:ro:" << msgs->motor_states[servo_ids.ro].load;
		//cout << "\nb:lo:" << msgs->motor_states[servo_ids.lo].load;
		if (msgs->motor_states[servo_ids.ro].load > -traction_lowbound
				|| msgs->motor_states[servo_ids.ro].load < -traction_upbound) {
			if ((abs(current_config.ro) < abs(max_config.ro)) && (abs(
					current_config.ro) > abs(min_config.ro))) {
				//cout<<"\nb:ro:"<<msgs->motor_states[servo_ids.ro].load;
				if ((msgs->motor_states[servo_ids.ro].load + traction_middle)
						> 0) {
					current_config.ro = current_config.ro + servo_angle_step;
				} else {
					current_config.ro = current_config.ro - servo_angle_step;
				}
			}
		}

		if (msgs->motor_states[servo_ids.lo].load > -traction_lowbound
				|| msgs->motor_states[servo_ids.lo].load < -traction_upbound) {
			if ((abs(current_config.lo) < abs(max_config.lo)) && (abs(
					current_config.lo) > abs(min_config.lo))) {
				if ((msgs->motor_states[servo_ids.lo].load + traction_middle)
						> 0) {
					current_config.lo = current_config.lo + servo_angle_step;
				} else {
					current_config.lo = current_config.lo - servo_angle_step;
				}
			}
		}
		set_Servo_Config(current_config);
	}
	//Traction control when moving right
	else if (robot_state == 3 && in_transition == 0) {
		//cout<<"\nb:ro:"<<msgs->motor_states[servo_ids.ro].load;
		//cout << "\nb:loF:" << msgs->motor_states[servo_ids.lo].load;
		//cout << "\nb:loA:" << msgs->motor_states[servo_ids.lo].position;
		if (msgs->motor_states[servo_ids.ro].load < traction_lowbound
				|| msgs->motor_states[servo_ids.ro].load > traction_upbound) {
			if ((abs(current_config.ro) < abs(max_config.ro)) && (abs(
					current_config.ro) > abs(min_config.ro))) {
				if ((msgs->motor_states[servo_ids.ro].load - traction_middle)
						> 0) {
					current_config.ro = current_config.ro + servo_angle_step;
				} else {
					current_config.ro = current_config.ro - servo_angle_step;
				}
			}

		}

		if (msgs->motor_states[servo_ids.lo].load < traction_lowbound
				|| msgs->motor_states[servo_ids.lo].load > traction_upbound) {
			if (abs(current_config.lo) < abs(max_config.lo) && abs(
					current_config.lo) > abs(min_config.lo)) {
				if ((msgs->motor_states[servo_ids.lo].load - traction_middle)
						> 0) {

					current_config.lo = current_config.lo + servo_angle_step;
				} else {
					current_config.lo = current_config.lo - servo_angle_step;
				}
			}
		}
		set_Servo_Config(current_config);
	}
}
void irData_Callback(const mother_ship::IRArray::ConstPtr &msg) {
	//	//ir_left_bottom, ir_left_top, ir_center, ir_right_bottom, ir_right_top
	//	int id = msg->radiation_type;
	//	ir_msg.data[id - 1] = msg->range;
	//	ir_data.publish(ir_msg);
	//	if (msg->range > 200)
	//		ir_state[id - 1] = 0;
	//	if (msg->range <= 200 && msg->range > 130)
	//		ir_state[id - 1] = 1;
	//	if (msg->range <= 130 && msg->range > 90)
	//		ir_state[id - 1] = 2;
	//	if (msg->range <= 90)
	//		ir_state[id - 1] = 3;
	//
	//	string status_str = "";
	//	for (int i = 0; i < 4; i++) {
	//		switch (ir_state[i]) {
	//		case 0:
	//			status_str += "Nothing  ";
	//			break;
	//		case 1:
	//			status_str += "Far      ";
	//			break;
	//		case 2:
	//			status_str += "Near     ";
	//			break;
	//		case 3:
	//			status_str += "In Front ";
	//			break;
	//		}
	//	}
	//	std_msgs::String str_msg;
	//	str_msg.data = status_str;
	//	ir_state_pub.publish(str_msg);
	//	//cout << "IR" << id << "Updated" << msg->range * (-1.0) << endl;
}

void imu_my_Callback(const mother_ship::MyIMU::ConstPtr &msg) {
	sensor_msgs::Imu imu_msg;
	boost::array<double, 9>
			cov = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	imu_msg.angular_velocity.x = msg->angleacc_x;
	imu_msg.angular_velocity.y = msg->angleacc_y;
	imu_msg.angular_velocity.z = msg->angleacc_z;
	imu_msg.angular_velocity_covariance = cov;
	imu_msg.linear_acceleration.x = msg->acc_x;
	imu_msg.linear_acceleration.y = msg->acc_y;
	imu_msg.linear_acceleration.z = msg->acc_z;
	imu_msg.linear_acceleration_covariance = cov;
	imu_msg.orientation.w = msg->gyro_w;
	imu_msg.orientation.x = msg->gyro_x;
	imu_msg.orientation.y = msg->gyro_y;
	imu_msg.orientation.z = msg->gyro_z;
	imu_msg.orientation_covariance = cov;

	//imu_data.publish(imu_msg);

	/*	tfScalar halfYaw = tfScalar(imu_msg.orientation.x / 180 * 3.14159)
	 * tfScalar(0.5);
	 tfScalar halfPitch = tfScalar(imu_msg.orientation.z / 180 * 3.14159)
	 * tfScalar(0.5);
	 tfScalar halfRoll = tfScalar(imu_msg.orientation.y / 180 * 3.14159)
	 * tfScalar(0.5);
	 tfScalar cosYaw = tfCos(halfYaw);
	 tfScalar sinYaw = tfSin(halfYaw);
	 tfScalar cosPitch = tfCos(halfPitch);
	 tfScalar sinPitch = tfSin(halfPitch);
	 tfScalar cosRoll = tfCos(halfRoll);
	 tfScalar sinRoll = tfSin(halfRoll);
	 sensor_msgs::Imu new_imu_msg;
	 new_imu_msg.orientation.x = cosRoll * sinPitch * cosYaw + sinRoll
	 * cosPitch * sinYaw;
	 new_imu_msg.orientation.y = cosRoll * cosPitch * sinYaw - sinRoll
	 * sinPitch * cosYaw;
	 new_imu_msg.orientation.z = sinRoll * cosPitch * cosYaw - cosRoll
	 * sinPitch * sinYaw;
	 new_imu_msg.orientation.w = cosRoll * cosPitch * cosYaw + sinRoll
	 * sinPitch * sinYaw;

	 tf::quaternionMsgToTF(new_imu_msg.orientation, tmp_);

	 tfScalar yaw, pitch, roll;
	 tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);

	 roll = -(180 - imu_msg.orientation.x) / 180.0 * 3.1415926;
	 pitch = (180 - imu_msg.orientation.y) / 180.0 * 3.1415926;
	 tmp_.setRPY(roll, pitch, 0.0);

	 transform_.setRotation(tmp_);

	 transform_.stamp_ = ros::Time::now();//imu_msg.header.stamp;

	 //ROS_INFO("Published imu transfrom");
	 tfB_->sendTransform(transform_);*/
}
